﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
//using Microsoft.Research.Kinect.Nui;
using Microsoft.Kinect;
using Kinect;
using System.Drawing;
using BPSimplified.Lib;
using System.IO;
using Imaging;

namespace KinectApplication
{
    public static class SkeletonParser
    {
        private const double blobDistanceTreshold = 100; // How far blob is from hand to be accepted
        private const int depthMinTreshold = -100;
        private const int depthMaxTreshold = 50;
        private const int depthTreshold = 1000; // Multiplied by -1 to +1
        private const int widthTreshold = 320; // Multiplied by -1 to +1, then add widthTreshold
        private const int heightTreshold = 240; // Multiplied by -1 to +1, then add heightTreshold
        private const int screenWidth = 640; // Should be twice the treshold, would be better if we'll check it from somewhere later
        private const int screenHeight = 480; // Should be twice the treshold, would be better if we'll check it from somewhere later
        private static List<Player> _latestPlayerData;

        public static bool SaveNextDepth = false;

        public static void ParseDepth(DepthImageFrame frame)
        {
            if (_latestPlayerData == null)
            {
                return;
            }

            if (frame == null)
            {
                return;
            }



            //DepthImageFrame frame = e.OpenDepthImageFrame();
            int arrayLength = frame.PixelDataLength;
            short[] depthArray = new short[arrayLength];
            frame.CopyPixelDataTo(depthArray);





            //short[] depthArray = frame.ToDepthArray();

            Debugging.ShowDepth(depthArray, 640, 480);

          //  Debugging.ShowDepth(frame.Image.Bits, 640, 480);

            Debugging.Clear();

          //  var hand = depthArray.Crop(75, 75, 50, 0, 0, 0);

           // Debugging.hand2.Source = hand.ConvertDepthFrameDataToBitmapData(75, 75).ToBitmapSource(75, 75);

            foreach (Player player in _latestPlayerData)
            {
                // Only 2 out of 6 players are tracked
                if (player.IsTracked)
                {
                   // resolveGesture(player.RightHand, depthArray, true);
                    resolveGesture(player.LeftHand, depthArray, false);

                   
                }
            }

        
            // TODO: replace with indirect call to unknown input
           // ReachMouseInput.InputEvent(_latestPlayerData);
        }

        public static void ParseSkeleton(SkeletonFrame skeletonGallery)
        {
            

            if (skeletonGallery == null)
            {
                return;
            }




            List<Player> playerData = new List<Player>();

            Skeleton[] skeletonCollection = new Skeleton[skeletonGallery.SkeletonArrayLength];
            skeletonGallery.CopySkeletonDataTo(skeletonCollection);

            foreach (Skeleton skeleton in skeletonCollection)
            {
                Player newPlayer = new Player();

                newPlayer.Id = skeleton.TrackingId;
                newPlayer.IsTracked = (skeleton.TrackingState == SkeletonTrackingState.Tracked);
                newPlayer.LeftHand = new Hand();
                newPlayer.LeftShoulder = new Joint();
                newPlayer.RightHand = new Hand();
                newPlayer.RightShoulder = new Joint();

                newPlayer.LeftHand.IsTracked = (skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked);
                newPlayer.LeftShoulder.IsTracked = (skeleton.Joints[JointType.ShoulderLeft].TrackingState == JointTrackingState.Tracked);
                newPlayer.RightHand.IsTracked = (skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked);
                newPlayer.RightShoulder.IsTracked = (skeleton.Joints[JointType.ShoulderRight].TrackingState == JointTrackingState.Tracked);

                newPlayer.LeftHand.X = skeleton.Joints[JointType.HandLeft].Position.X;
                newPlayer.LeftHand.Y = skeleton.Joints[JointType.HandLeft].Position.Y;
                newPlayer.LeftHand.Z = skeleton.Joints[JointType.HandLeft].Position.Z;

                newPlayer.RightHand.X = skeleton.Joints[JointType.HandRight].Position.X;
                newPlayer.RightHand.Y = skeleton.Joints[JointType.HandRight].Position.Y;
                newPlayer.RightHand.Z = skeleton.Joints[JointType.HandRight].Position.Z;

                newPlayer.LeftShoulder.X = skeleton.Joints[JointType.ShoulderLeft].Position.X;
                newPlayer.LeftShoulder.Y = skeleton.Joints[JointType.ShoulderLeft].Position.Y;
                newPlayer.LeftShoulder.Z = skeleton.Joints[JointType.ShoulderLeft].Position.Z;

                newPlayer.RightShoulder.X = skeleton.Joints[JointType.ShoulderRight].Position.X;
                newPlayer.RightShoulder.Y = skeleton.Joints[JointType.ShoulderRight].Position.Y;
                newPlayer.RightShoulder.Z = skeleton.Joints[JointType.ShoulderRight].Position.Z;

                newPlayer.LeftHand.Vector = skeleton.Joints[JointType.HandLeft].Position;
                newPlayer.RightHand.Vector = skeleton.Joints[JointType.HandRight].Position;
                newPlayer.LeftShoulder.Vector = skeleton.Joints[JointType.ShoulderLeft].Position;
                newPlayer.RightShoulder.Vector = skeleton.Joints[JointType.ShoulderRight].Position;

                playerData.Add(newPlayer);
            }

            _latestPlayerData = playerData;
        }

        // TODO: temp helper 
        //private static bool gestureRecording = false;
        private static void resolveGesture(Hand targetHand, short[] depthArray, bool isRight)
        {
            if (targetHand.IsTracked)
            {
                /*
                //singleHand.HandGesture
                int handX = (int)((targetHand.X * widthTreshold) + widthTreshold);
                int handY = (int)((-targetHand.Y * heightTreshold) + heightTreshold);

                short handZ = (short)(targetHand.Z * depthTreshold);

                short handZmin = (short)(handZ + depthMinTreshold);
                short handZmax = (short)(handZ + depthMaxTreshold);
                */
               
                float handy, handx;
                //short handz;
                DepthImagePoint handDepthPoint;

                //Kinect._runtime. .SkeletonEngine.SkeletonToDepthImage(targetHand.Vector, out handx, out handy, out handz);
                handDepthPoint = Kinect._runtime.MapSkeletonPointToDepth(targetHand.Vector, DepthImageFormat.Resolution640x480Fps30);


                handx = handDepthPoint.X;
                handy = handDepthPoint.Y;
                int handZ = handDepthPoint.Depth;


                //handx = Math.Max(0, Math.Min(handX * screenWidth, screenWidth));
                //handy = Math.Max(0, Math.Min(handY * screenHeight, screenHeight));

                /*
                if (SaveNextDepth == true)
                {
                    SaveNextDepth = false;

                    

                    using (FileStream fs = new FileStream(@"E:\Koulu\Gradu\X" + (int)handx + "-Y" + (int)handy + "-Z" + handZ + ".dat", FileMode.OpenOrCreate, FileAccess.Write))
                    {
                        using (BinaryWriter bw = new BinaryWriter(fs))
                        {
                            foreach (short value in depthArray)
                            {
                                bw.Write(value);
                            }
                        }
                    }

                }
                */

                // TODO: Left blob debugging, temp
                //  if (!isRight)
                //  Debugging.ShowHand(50, 50, handX, handY);

                //    Debugging.ShowBlobs(blobs);


                //foreach (Blob blob in blobs)
                //{
                //    if (blob.Area > area)
                //    {
                //        Point center = blob.CenterOfGravity;
                //        int dx = center.X - handX;
                //        int dy = center.Y - handY;
                //        if (dx < 0) { dx = -dx; }
                //        if (dy < 0) { dy = -dy; }
                //        double newDistance = Math.Sqrt((dx * dx) + (dy * dy));

                //        if (newDistance < blobDistanceTreshold)
                //        {
                //            area = blob.Area;
                //            bestBlob = blob;
                //        }
                //    }
                //}

               
                //handz = (short)ImageProcessing.RawDepthToMeters(handz);

               // var hand = depthArray.Crop(100, 100, (int)handx, (int)handy, handZ - 300, handZ + 50);

              //  Debugging.hand2.Source = hand.ConvertDepthFrameDataToBitmapData(100, 100).ToBitmapSource(100, 100);

                //Blob gestureBlob = ImageProcessing.ExtractTheBlob(hand);
               // Blob gestureBlob = ImageProcessing.ExtractOneBlob(depthArray, (int)handx, (int)handy, 640, 480);


                QueueLinearFloodFiller floodFiller = new QueueLinearFloodFiller(depthArray, 640, 480, 6666, true, 50);

                floodFiller.FloodFill(new System.Drawing.Point(640 - (int)handx,(int)handy));



                var hand = depthArray.Crop(floodFiller.Left, floodFiller.Top, floodFiller.Width2, floodFiller.Height2, handZ - 300, handZ + 50);
                Debugging.hand2.Source = hand.ConvertDepthFrameDataToBitmapData(floodFiller.Width2, floodFiller.Height2).ToBitmapSource(floodFiller.Width2, floodFiller.Height2);

                Debugging.ShowHand(25, 25, (int)handx, (int)handy);

                Debugging.ShowRectangle(floodFiller.Left, floodFiller.Top, floodFiller.Width2, floodFiller.Height2);

               // Debugging.ShowBlob(gestureBlob, handx, handy);

               // if ((gestureBlob != null) && (gestureBlob.Height > 0))
               // {
                  //  short[] handArray = depthArray.BlobArray(gestureBlob, handz - 200, handz + 200);
                  //  short[] resizedHandArray = handArray.ResizeBilinear((int)gestureBlob.Width, (int)gestureBlob.Height, HandGestureReader.PicWidth, HandGestureReader.PicHeight);

                    /*
                    if (!gestureRecording)
                    {
                        gestureRecording = true;
                        HandGestureReader.SetLearnGesture(HandGestures.Neutral);
                    }
                    HandGestureReader.LearnGestureEvent(true, resizedHandArray);
                    */

                  //  HandGestureReader.IdentifyGesture(isRight, resizedHandArray, targetHand);
               // }
            }
        }

        public static void ShowDebug(short[] depthArray, int handx, int handy, int handz)
        {

            var hand = depthArray.Crop(100, 100, (int)handx, (int)handy, handz - 300, handz + 50);

            Debugging.hand2.Source = hand.ConvertDepthFrameDataToBitmapData(100, 100).ToBitmapSource(100, 100);

            //Blob gestureBlob = ImageProcessing.ExtractTheBlob(hand);
            Blob gestureBlob = ImageProcessing.ExtractOneBlob(depthArray, (int)handx, (int)handy, 640, 480);



            Debugging.image.Source = ImageProcessing.depthArray.ConvertDepthFrameDataToBitmapData(640, 480).ToBitmapSource(640, 480);

            Debugging.ShowHand(25, 25, (int)handx, (int)handy);

            Debugging.ShowBlob(gestureBlob, handx, handy);

        }
        
    }
}
